
/**
 * @file CSDetectorLC.hpp
 * General Solver.
 */

//============================================================================
//
//  Revision
//
//  2019/08/07
//  create a general solver for multi-gpss ppp solution
//  shjzhang
//
//  2020/01/16
//  remove stateMap/covMap, which makes the program complicated
//  shjzhang.
//
//============================================================================


#include "CSDetectorLC.hpp"
#include "SystemTime.hpp"
#include "Ambiguity.hpp"
#include "NEUUtil.hpp"

#define debug 0

using namespace gpstk::StringUtils;

namespace gpstk
{

      // Index initially assigned to this class
   int CSDetectorLC::classIndex = 9600000;

      // Returns an index identifying this object.
   int CSDetectorLC::getIndex() const
   { return index; }

      // Returns a string identifying this object.
   std::string CSDetectorLC::getClassName() const
   { return "CSDetectorLC"; }

   void CSDetectorLC::Init(void)
   {
       // set up variables and equations for PPP

       // Warning: not consider the GLONASS system until now
       // Warning: only consider LC combinations
       // Warning: see CSDetectorLCUC for UC combinations

       //////////////////////////////////////////////////////////////
       //
       // please augument for GAL/BDS system
       // 
       // wavelengthBLC12 = clight/(f1+f2)
       //
       // BLC12  = clight/(f1+f2)[( B1 + f2/(f1-f2)*Nw]
       // BLC15  = clight/(f1+f5)[( B1 + f5/(f1-f5)*Nw)]
       //
       //////////////////////////////////////////////////////////////

   }

   void CSDetectorLC::setUpEquations(void)
   {

       ///////////////////////////////////////////////
       //
       // Now, define variables need to be estimated 
       // 
       ///////////////////////////////////////////////
       
          // These variables are, by default, SourceID-indexed
       Variable dx( TypeID::dStaX );
       Variable dy( TypeID::dStaY );
       Variable dz( TypeID::dStaZ );

       VariableSet xyzVarSet;
       xyzVarSet.insert(dx);
       xyzVarSet.insert(dy);
       xyzVarSet.insert(dz);

       Variable dt( TypeID::dcdtSta );

       //////////////////////////////
       // Define Equation object for code/phase observations, and add variables 
       //////////////////////////////

       // GPS LC
       Equation equLC12GPS( TypeID::diffPrefitL12 );
       Equation equLC15GPS( TypeID::diffPrefitL15 );

       // GAL LC
       Equation equLC15GAL( TypeID::diffPrefitL15, SatelliteSystem::Galileo );
       Equation equLC17GAL( TypeID::diffPrefitL17, SatelliteSystem::Galileo );
       Equation equLC18GAL( TypeID::diffPrefitL18, SatelliteSystem::Galileo );
       Equation equLC16GAL( TypeID::diffPrefitL16, SatelliteSystem::Galileo );

       // BDS LC                    
       Equation equLC27BDS( TypeID::diffPrefitL27, SatelliteSystem::BDS );
       Equation equLC26BDS( TypeID::diffPrefitL26, SatelliteSystem::BDS );

       /////////////////////////
       // insert variables into equations
       /////////////////////////

       // GPS LC
       equLC12GPS.addVarSet(xyzVarSet); 
       equLC15GPS.addVarSet(xyzVarSet); 

       // GAL LC
       equLC15GAL.addVarSet(xyzVarSet); 
       equLC17GAL.addVarSet(xyzVarSet); 
       equLC18GAL.addVarSet(xyzVarSet); 
       equLC16GAL.addVarSet(xyzVarSet); 

       // BDS LC
       equLC27BDS.addVarSet(xyzVarSet); 
       equLC26BDS.addVarSet(xyzVarSet); 

       // Insert dt 
       // GPS
       equLC12GPS.addVariable(dt, true);  
       equLC15GPS.addVariable(dt, true);  

       // GAL
       equLC15GAL.addVariable(dt, true); 
       equLC17GAL.addVariable(dt, true); 
       equLC18GAL.addVariable(dt, true); 
       equLC16GAL.addVariable(dt, true); 

       // BDS LC
       equLC27BDS.addVariable(dt, true); 
       equLC26BDS.addVariable(dt, true); 

       // define the weight for all combinations relative PC12GPS

          // Phase equations should have higher relative weight
       equLC12GPS.setWeight(1.0);              
       equLC15GPS.setWeight(1.0);                     

       equLC15GAL.setWeight(1.0); 
       equLC17GAL.setWeight(1.0); 
       equLC18GAL.setWeight(1.0); 
       equLC16GAL.setWeight(1.0); 

       equLC27BDS.setWeight(1.0); 
       equLC26BDS.setWeight(1.0); 

          ///////////
          // SETTING THE RULES: SETUP EQUATION SYSTEM
          ///////////
       
       // YOU MUST clear the equation system firstly!!!!!
       // only current equations is feed to equSystem
       equSystem.clearEquations();
       
          // Add equation descriptions
       if(usingGPS)
       {
           for(TypeIDSet::const_iterator typeIt=gpsCombTypeSet.begin();
                   typeIt!=gpsCombTypeSet.end();
                   typeIt++)
           {
               if( (*typeIt) == TypeID::LC12 )
               {
                   equSystem.addEquation(equLC12GPS);
               }
               else if ( (*typeIt) == TypeID::LC15 )
               {
                   equSystem.addEquation(equLC15GPS);
               }
           }
       }

       if(usingGAL)
       {
           for(TypeIDSet::const_iterator typeIt=galCombTypeSet.begin();
                   typeIt!=galCombTypeSet.end();
                   typeIt++)
           {
               if ( (*typeIt) == TypeID::LC15 )
               {
                   equSystem.addEquation(equLC15GAL);
               }
               else if ( (*typeIt) == TypeID::LC17 )
               {
                   equSystem.addEquation(equLC17GAL);
               }
               else if ( (*typeIt) == TypeID::LC18 )
               {
                   equSystem.addEquation(equLC18GAL);
               }
               else if ( (*typeIt) == TypeID::LC16 )
               {
                   equSystem.addEquation(equLC16GAL);
               }
           }
       }

       if(usingBDS)
       {
           for(TypeIDSet::const_iterator typeIt=bdsCombTypeSet.begin();
                   typeIt!=bdsCombTypeSet.end();
                   typeIt++)
           {
               if( (*typeIt) == TypeID::LC27 )
               {
                   equSystem.addEquation(equLC27BDS);
               }
               else if ( (*typeIt) == TypeID::LC26 )
               {
                   equSystem.addEquation(equLC26BDS);
               }
           }
       }

   } // end of setUpEquations



      /* Code to be executed before 'Compute()' method.
       *
       * @param gData    Data object holding the data.
       */
   gnssRinex& CSDetectorLC::Process( gnssRinex& gRin)
       noexcept(false)
   {
      try
      {
          if(debug)
          {
              cout << "CSDetectorLC" << endl;
              cout << YDSTime(gRin.header.epoch) << endl;
          }

          // set-up equtions firstly for equation systems
          setUpEquations();


          // let's compute the time-difference data for cycle-slip detection
          getEpochDiffData(gRin);

          if(debug)
             gRin.dump(cout, 1);

          // store the satTypeValueMap
          stvDataPrev = gRin.body;

          // keep only the data which is used in the time-difference xyzt esimate
          TypeIDSet typeSet;

          // GPS
          typeSet.insert(TypeID::diffPrefitL12);
          typeSet.insert(TypeID::diffPrefitL15);

          // GAL
          typeSet.insert(TypeID::diffPrefitL15);
          typeSet.insert(TypeID::diffPrefitL17);
          typeSet.insert(TypeID::diffPrefitL18);
          typeSet.insert(TypeID::diffPrefitL16);

          // BDS
          typeSet.insert(TypeID::diffPrefitL27);
          typeSet.insert(TypeID::diffPrefitL26);

          typeSet.insert(TypeID::dStaX);
          typeSet.insert(TypeID::dStaY);
          typeSet.insert(TypeID::dStaZ);
          typeSet.insert(TypeID::dcdtSta);

          typeSet.insert(TypeID::weight);
          typeSet.insert(TypeID::CSFlag);

          gnssRinex gBack = gRin;
          gBack.keepOnlyTypeID(typeSet);

          if(firstTime)
          {
              vtpvAcc = 0.0;
              numObsAcc = 0;
              firstTime = false;
              return gRin;
          }

          SatIDSet satRejectedSet;
          for(auto stv: gBack.body)
          {
              if(stv.second.find(TypeID::CSFlag)!=stv.second.end())
              {
                  if(stv.second[TypeID::CSFlag] == 1.0)
                  {
                      satRejectedSet.insert(stv.first);
                  }
              }
          }

          // remove satellite which are already flagged as cycle-slips
          gBack.removeSatID(satRejectedSet);

          if(gBack.numSats() < 4)
          {
              return gRin;
          }

             // Prepare the equation system with current data
          equSystem.Prepare(gBack);

             // Get the number of unknowns being processed
          int numEqu( equSystem.getEquNumber() );
          int numUnknowns( equSystem.getTotalNumVariables() );

             // Get the set with unknowns being processed
          currentUnkSet =  equSystem.getVarUnknowns() ;


          std::list<Equation> currentEquList;
          currentEquList = equSystem.getCurrentEquationsList();

          if(debug)
          {
             for(std::list<Equation>::iterator it=currentEquList.begin();
                 it!=currentEquList.end();
                 ++it)
             {
                 (*it).dump(cout);
             }
          }

             // Get matrices and vectors out of equation system
             // Measurements vector (Prefit-residuals)
          measVector = equSystem.getPrefitsVector();
          hMatrix = equSystem.getGeometryMatrix();
          rMatrix = equSystem.getWeightsMatrix();


          /////////////////////////
          // apriori coordinate constraint
          /////////////////////////

          // if 1, means cycle slip happened, else it's good one
          // warning, 
          // the dimension of csVec is equal with numEqu, don't include the
          // extended constraint equation
          Vector<int> csVec(numEqu, 0);

          Vector<double> postfitResiduals;
          Vector<double> standResidual;
          double sig;

          // get variable to take out the position index 
          Variable varDX = getVariable(TypeID::dStaX,   currentUnkSet);
          Variable varDY = getVariable(TypeID::dStaY,   currentUnkSet);
          Variable varDZ = getVariable(TypeID::dStaZ,   currentUnkSet);
          Variable varDT = getVariable(TypeID::dcdtSta, currentUnkSet);

          int idxDX = varDX.getNowIndex();
          int idxDY = varDY.getNowIndex();
          int idxDZ = varDZ.getNowIndex();
          int idxDT = varDT.getNowIndex();

          int numApriori;

          Vector<double> xhatminus(numUnknowns, 0.0);
          Matrix<double> Pminus(numUnknowns, numUnknowns, 0.0);

          // the aprioir-variance for xhatminus, 
          // if applyDXConstr, sigDX will be a very small value
          // if applyDTConstr, sigDT will be a very small value
          Pminus(idxDX, idxDX) = sigDX*sigDX;
          Pminus(idxDY, idxDY) = sigDY*sigDY;
          Pminus(idxDZ, idxDZ) = sigDZ*sigDZ;
          Pminus(idxDT, idxDT) = sigDT*sigDT;

          if(applyDXConstr && applyDTConstr)
          {
              numApriori = 4;
          }
          else if(applyDXConstr && !applyDTConstr)
          {
              numApriori = 3;
          }
          else if( !applyDXConstr && applyDTConstr)
          {
              numApriori = 1;
          }
          else
          {
              numApriori = 0;
          }

          if(debug)
          {
              cout << "useDIA:" << useDIA << endl;
              cout << "measVector:" << measVector << endl;
          }

          if(useDIA)
          {
              DIA( numApriori,
                   xhatminus,
                   Pminus,
                   measVector, 
                   hMatrix, 
                   rMatrix, 
                   postfitResiduals, 
                   standResidual, 
                   sig,
                   csVec); 
          }
          else
          {
              // now, get solution using robus-least square
              Robust(numApriori,
                     xhatminus, 
                     Pminus, 
                     measVector, 
                     hMatrix, 
                     rMatrix, 
                     postfitResiduals, 
                     standResidual, 
                     sig,
                     csVec); 
          }


          if(debug)
          {
              cout << "postfitResiduals" << endl;
              cout << postfitResiduals << endl;
              cout << "cs:" << endl;
              cout << csVec << endl;
              cout << "solution" << endl;
              cout << solution << endl;
          }


          int i(0);
          for(auto equ: currentEquList)
          {
              SatID equSat = equ.header.equationSat;
              Variable equVar = equ.getIndependentTerm();
              TypeID equType = equVar.getType();

              // insert cycle slip flags into data 
              if(csVec(i) == 1)
              {
                  gRin.body[equSat][TypeID::CSFlag] = 1.0;
                  gRin.body[equSat][TypeID::CSLC] = 1.0;
              }
              else
              {
                  gRin.body[equSat][TypeID::CSFlag] = 0.0;
                  gRin.body[equSat][TypeID::CSLC] = 0.0;
              }

              gRin.body[equSat][TypeID::sigma] = sig;

              // insert difference postfit residuals into data structures
              if(equType == TypeID::diffPrefitL12)
              {
                  gRin.body[equSat][TypeID::diffPostfitL12] = postfitResiduals(i);
                  gRin.body[equSat][TypeID::diffStandResL12] = standResidual(i);
              }
              else if(equType == TypeID::prefitL15)
              {
                  gRin.body[equSat][TypeID::diffPostfitL15] =  postfitResiduals(i);
                  gRin.body[equSat][TypeID::diffStandResL15] = standResidual(i);
              }
              else if(equType == TypeID::prefitL17)
              {
                  gRin.body[equSat][TypeID::diffPostfitL17] =  postfitResiduals(i);
                  gRin.body[equSat][TypeID::diffStandResL17] = standResidual(i);
              }
              else if(equType == TypeID::prefitL18)
              {
                  gRin.body[equSat][TypeID::diffPostfitL18] =  postfitResiduals(i);
                  gRin.body[equSat][TypeID::diffStandResL18] = standResidual(i);
              }
              else if(equType == TypeID::prefitL16)
              {
                  gRin.body[equSat][TypeID::diffPostfitL16] =  postfitResiduals(i);
                  gRin.body[equSat][TypeID::diffStandResL16] = standResidual(i);
              }
              else if(equType == TypeID::prefitL27)
              {
                  gRin.body[equSat][TypeID::diffPostfitL27] =  postfitResiduals(i);
                  gRin.body[equSat][TypeID::diffStandResL27] = standResidual(i);
              }
              else if(equType == TypeID::prefitL26)
              {
                  gRin.body[equSat][TypeID::diffPostfitL26] =  postfitResiduals(i);
                  gRin.body[equSat][TypeID::diffStandResL26] = standResidual(i);
              }

              // index
              i++;
          }

          satTypeValueMap tempSatData = gRin.body;

          TypeIDSet typeSetOut;
          typeSetOut.insert(TypeID::CSLC);
          typeSetOut.insert(TypeID::sigma);
          typeSetOut.insert(TypeID::diffPostfitL12);
          typeSetOut.insert(TypeID::diffPostfitL15);
          typeSetOut.insert(TypeID::diffPostfitL17);
          typeSetOut.insert(TypeID::diffPostfitL18);
          typeSetOut.insert(TypeID::diffPostfitL16);
          typeSetOut.insert(TypeID::diffPostfitL27);
          typeSetOut.insert(TypeID::diffPostfitL26);

          typeSetOut.insert(TypeID::diffStandResL12);
          typeSetOut.insert(TypeID::diffStandResL15);
          typeSetOut.insert(TypeID::diffStandResL17);
          typeSetOut.insert(TypeID::diffStandResL18);
          typeSetOut.insert(TypeID::diffStandResL16);
          typeSetOut.insert(TypeID::diffStandResL27);
          typeSetOut.insert(TypeID::diffStandResL26);

          tempSatData.keepOnlyTypeID(typeSetOut);

          if(pCSStream!=NULL)
          {


              for(auto stv: tempSatData)
              {
                  (*pCSStream)
                      << YDSTime(gRin.header.epoch) << " "
                      << stv.first << " "
                      << stv.second << " "
                      << endl;
              }
          }

          if(pSolStream!=NULL)
          {
              (*pSolStream) 
                  << YDSTime(gRin.header.epoch) << " "
                  << solution << endl;
          }

          // reset filter
          CommonTime epoch( gRin.header.epoch );

      }
      catch(Exception& u)
      {
            // Throw an exception if something unexpected happens
         ProcessingException e( getClassName() + ":"
                                + StringUtils::asString( getIndex() ) + ":"
                                + u.what() );

         GPSTK_THROW(e);

      }

      return gRin;

   }  // End of method 'CSDetectorLC::preCompute()'


      // Correct the state vector and covariance matrix
      //
      // @param gData    Data object holding the data.
      //
   int CSDetectorLC::DIA( int numApriori,
                          Vector<double>& xhatminus,
                          Matrix<double>& Pminus,
                          Vector<double>& prefitResiduals,
                          Matrix<double>& designMatrix,
                          Matrix<double>& weightMatrix,
                          Vector<double>& postfitResiduals,
                          Vector<double>& standResidual,
                          double& sig,
                          Vector<int>& outlierVector)
       noexcept(false)
   {

         // By default, results are invalid
      if (!(weightMatrix.isSquare()))
      {
         InvalidSolver e("Compute(): Weight matrix is not square");
         cerr << e << endl;
         exit(-1);
      }

      int wRow = static_cast<int>(weightMatrix.rows());
      int pRow = static_cast<int>(prefitResiduals.size());
      if (!(wRow==pRow))
      {
         InvalidSolver e("Compute(): size does not match!");
         cerr << e << endl;
         exit(-1);
      }


      int gRow = static_cast<int>(designMatrix.rows());
      if (!(gRow==pRow))
      {
         InvalidSolver e("Compute(): size does not match !");
         cerr << e << endl;
         exit(-1);
      }

      int gCol = static_cast<int>(designMatrix.cols());
      int aprioriStateRow = static_cast<int>(xhatminus.size());
      if (!(gCol==aprioriStateRow))
      {
         InvalidSolver e("MeasUpdate(): designMatrix size and a priori state \
estimation vector do not match.");
         GPSTK_THROW(e);
      }

      int pMCol = static_cast<int>(Pminus.cols());
      if ( pMCol != aprioriStateRow )
      {
         InvalidSolver e("MeasUpdate(): Sizes of a priori error covariance \
matrix and a priori state estimation vector do not match.");
         GPSTK_THROW(e);
      }

         // After checking sizes, let's do the real correction work
      Matrix<double> invPMinus;
      Matrix<double> AT( transpose(designMatrix) );

      try
      {
         invPMinus = inverseChol(Pminus);
      }
      catch(...)
      {
         InvalidSolver e("MeasUpdate(): Unable to compute invPMinus matrix.");
         GPSTK_THROW(e);
         return -1;
      }

      int numEqu( equSystem.getEquNumber() );
      int numUnknowns( equSystem.getTotalNumVariables() );

      // Covariance for residual V;
      Matrix<double> Qvv(pRow, pRow, 0.0);

      Matrix<double> tempWeight = weightMatrix;
      Vector<int> outlierVec(numEqu, 0);

      Vector<double> stdResidual(pRow, 0.0);
      Vector<double> robustResidual(pRow, 0.0);

      double VTPV;
      int numValid = numEqu;

      double maxResidual;
      int iter = 0;
      double sigma;

      // one percent threshod for standard normal distribution
      double threshold = 3.5 ; 

      do 
      {
          try
          {
             Matrix<double> invTemp;
             invTemp = ( AT * tempWeight* designMatrix + invPMinus );

                // Compute the a posteriori error covariance matrix
             covMatrix = inverseChol( invTemp );

          }
          catch(...)
          {
             InvalidSolver e("MeasUpdate(): Unable to compute P matrix.");
             GPSTK_THROW(e);
             return -1;
          }

          try
          {
                // Compute the a posteriori state estimation
              solution = covMatrix * ( (AT * tempWeight * prefitResiduals) + (invPMinus * xhatminus) );
          }
          catch(Exception e)
          {
             InvalidSolver eis("MeasUpdate(): Unable to compute xhat.");
             GPSTK_THROW(eis);
             return -1;
          }

             // Compute the postfit residuals Vector
          postfitResiduals = prefitResiduals - (designMatrix * solution);

          if(debug)
          {
             cout << "postfitResiduals" << endl;
             cout <<  postfitResiduals  << endl;
          }

          VTPV = 0.0;
          for( int i = 0; i < pRow; ++i)
          {
              VTPV += postfitResiduals(i)*postfitResiduals(i)*tempWeight(i,i);
          }

          Vector<double> Vx = solution - xhatminus;

          double VTPVX(0.0);
          for(int i=0; i< numUnknowns; i++ )
          {
              VTPVX += Vx(i)*invPMinus(i,i)*Vx(i);
          }

          // update the weight
          // warning, the numvalid should be valid number
          sigma = std::sqrt( (VTPV + VTPVX)/(numValid + numApriori - numUnknowns) );

          // let's compute the population variance
          double tempVTPVAcc;
          double tempNumObsAcc;

          tempVTPVAcc = vtpvAcc + VTPV;
          tempNumObsAcc = numObsAcc + numValid - 4;

          double totalSigma = std::sqrt(tempVTPVAcc/(tempNumObsAcc));

          Qvv = inverseChol(tempWeight) - designMatrix * covMatrix * AT;

          for(int i=0; i<pRow; i++)
          {
              stdResidual[i] =  std::abs( postfitResiduals(i) /(sigma*std::sqrt(std::abs(Qvv(i,i)))) ) ;
          }

          for(int i=0; i<pRow; i++)
          {
              robustResidual[i] =  std::abs( postfitResiduals(i) /(totalSigma*std::sqrt(Qvv(i,i))) ) ;
          }

          int indexMax = getMaxIndex(stdResidual, outlierVec);

          if(ZTest)
          {
              maxResidual = robustResidual(indexMax);
          }
          else 
          {
              maxResidual = stdResidual(indexMax);
          }

          if(debug)
          {
             cout << "robustResidual" << endl;
             cout <<  robustResidual << endl;
          }


          // apply a very small value for the outlier
          if( maxResidual > threshold )
          {
              tempWeight(indexMax, indexMax) = 1.0E-10;

              // update the flag for the outlier
              outlierVec(indexMax) = 1;

              // each time, one outlier will be removed
              numValid--;
          }

          iter++;

      } while( maxResidual > threshold && numValid > (numUnknowns - numApriori) );

      // at last, solve the solution/covMatrix at last
      try
      {
         Matrix<double> invTemp;
         invTemp = ( AT * tempWeight* designMatrix + invPMinus );

            // Compute the a posteriori error covariance matrix
         covMatrix = inverseChol( invTemp );

      }
      catch(...)
      {
         InvalidSolver e("MeasUpdate(): Unable to compute P matrix.");
         GPSTK_THROW(e);
         return -1;
      }

      try
      {
            // Compute the a posteriori state estimation
          solution = covMatrix * ( (AT * tempWeight * prefitResiduals) + (invPMinus * xhatminus) );
      }
      catch(Exception e)
      {
         InvalidSolver eis("MeasUpdate(): Unable to compute xhat.");
         GPSTK_THROW(eis);
         return -1;
      }

         // Compute the postfit residuals Vector
      postfitResiduals = prefitResiduals - (designMatrix * solution);

      VTPV = 0.0;
      for( int i = 0; i < pRow; ++i)
      {
          VTPV += postfitResiduals(i)*postfitResiduals(i)*tempWeight(i,i);
      }

      Vector<double> Vx = solution - xhatminus;

      double VTPVX(0.0);
      for(int i=0; i< numUnknowns; i++ )
      {
          VTPVX += Vx(i)*invPMinus(i,i)*Vx(i);
      }

      // update the weight
      // warning, the numvalid should be valid number
      sigma = std::sqrt( (VTPV + VTPVX)/(numValid + numApriori - numUnknowns) );

      // let's compute the population variance
      double tempVTPVAcc;
      double tempNumObsAcc;

      tempVTPVAcc = vtpvAcc + VTPV;
      tempNumObsAcc = numObsAcc + numValid - 4;

      double totalSigma = std::sqrt(tempVTPVAcc/(tempNumObsAcc));

      Qvv = inverseChol(tempWeight) - designMatrix * covMatrix * AT;

      for(int i=0; i<pRow; i++)
      {
          stdResidual[i] =  std::abs( postfitResiduals(i) /(sigma*std::sqrt(std::abs(Qvv(i,i)))) ) ;
      }

      for(int i=0; i<pRow; i++)
      {
          robustResidual[i] =  std::abs( postfitResiduals(i) /(totalSigma*std::sqrt(Qvv(i,i))) ) ;
      }

      ///////////////////////
      // return
      ///////////////////////

      // local sigma and standResidual
      sig = sigma;
      standResidual = stdResidual;

      // store the total number of the freedom
      vtpvAcc += VTPV;
      numObsAcc += numValid - 4;

      // return the outlier flags
      outlierVector = outlierVec;

      return 0;

   }  // End of method 'CSDetectorLC::MeasUpdate()'


      // Correct the state vector and covariance matrix
      //
      // @param gData    Data object holding the data.
      //
   int CSDetectorLC::Robust( int numApriori,
                             Vector<double>& xhatminus,
                             Matrix<double>& Pminus,
                             Vector<double>& prefitResiduals,
                             Matrix<double>& designMatrix,
                             Matrix<double>& weightMatrix,
                             Vector<double>& postfitResiduals,
                             Vector<double>& standResidual,
                             double& sig,
                             Vector<int>& outlierVector)
       noexcept(false)
   {

         // By default, results are invalid
      if (!(weightMatrix.isSquare()))
      {
         InvalidSolver e("Compute(): Weight matrix is not square");
         cerr << e << endl;
         exit(-1);
      }

      int wRow = static_cast<int>(weightMatrix.rows());
      int pRow = static_cast<int>(prefitResiduals.size());
      if (!(wRow==pRow))
      {
         InvalidSolver e("Compute(): size does not match!");
         cerr << e << endl;
         exit(-1);
      }


      int gRow = static_cast<int>(designMatrix.rows());
      if (!(gRow==pRow))
      {
         InvalidSolver e("Compute(): size does not match !");
         cerr << e << endl;
         exit(-1);
      }

      int gCol = static_cast<int>(designMatrix.cols());
      int aprioriStateRow = static_cast<int>(xhatminus.size());
      if (!(gCol==aprioriStateRow))
      {
         InvalidSolver e("MeasUpdate(): designMatrix size and a priori state \
estimation vector do not match.");
         GPSTK_THROW(e);
      }

      int pMCol = static_cast<int>(Pminus.cols());
      if ( pMCol != aprioriStateRow )
      {
         InvalidSolver e("MeasUpdate(): Sizes of a priori error covariance \
matrix and a priori state estimation vector do not match.");
         GPSTK_THROW(e);
      }

         // After checking sizes, let's do the real correction work
      Matrix<double> invPMinus;
      Matrix<double> AT( transpose(designMatrix) );

      try
      {
         invPMinus = inverseChol(Pminus);
      }
      catch(...)
      {
         InvalidSolver e("MeasUpdate(): Unable to compute invPMinus matrix.");
         GPSTK_THROW(e);
         return -1;
      }

      int numEqu( prefitResiduals.size() );
      int numUnknowns( designMatrix.cols() );

      int numValid = numEqu;

      Matrix<double> tempWeight = weightMatrix;

      Vector<double> stdResidual(pRow, 0.0);
      Vector<double> robustResidual(pRow, 0.0);

      double VTPV;
      double sigma;

      /////////////////////
      // step 1: Solve the robust x with robust observation
      /////////////////////

      try
      {
         Matrix<double> invTemp;
         invTemp = ( AT * tempWeight* designMatrix + invPMinus );
         covMatrix = inverseChol( invTemp );

      }
      catch(...)
      {
         InvalidSolver e("MeasUpdate(): Unable to compute P matrix.");
         GPSTK_THROW(e);
         return -1;
      }

      try
      {
            // Compute the a posteriori state estimation
          solution = covMatrix * ( (AT * tempWeight * prefitResiduals) + (invPMinus * xhatminus) );
      }
      catch(Exception e)
      {
         InvalidSolver eis("MeasUpdate(): Unable to compute xhat.");
         GPSTK_THROW(eis);
         return -1;
      }

         // Compute the postfit residuals Vector
      postfitResiduals = prefitResiduals - (designMatrix * solution);

      VTPV = 0.0;
      for( int i = 0; i < pRow; ++i)
      {
          VTPV += postfitResiduals(i)*postfitResiduals(i)*tempWeight(i,i);
      }

      Vector<double> Vx = solution - xhatminus;

      double VTPVX(0.0);
      for(int i=0; i< numUnknowns; i++ )
      {
          VTPVX += Vx(i)*invPMinus(i,i)*Vx(i);
      }

      // update the weight
      // warning, the numvalid should be valid number

      sigma = std::sqrt( (VTPV + VTPVX)/(numValid + numApriori - numUnknowns) );

      // Covariance for observation;
      Matrix<double> Qvv(pRow, pRow, 0.0);

      // warning: here the weightMatrix should be given for all observations
      Qvv = inverseChol(weightMatrix) - designMatrix * covMatrix * AT;

      for(int i=0; i<pRow; i++)
      {
          stdResidual[i] =  std::abs( postfitResiduals(i) /(sigma*std::sqrt(Qvv(i,i))) ) ;
      }

      /*
       * let's compute the robutst variance and residual
       */
      double tempVTPVAcc;
      double tempNumObsAcc;

      tempVTPVAcc = vtpvAcc + VTPV;
      tempNumObsAcc = numObsAcc + numValid - 4;

      double totalSigma = std::sqrt(tempVTPVAcc/(tempNumObsAcc));

      Qvv = inverseChol(weightMatrix);

      for(int i=0; i<pRow; i++)
      {
          robustResidual[i] =  std::abs( postfitResiduals(i) /(totalSigma*std::sqrt(Qvv(i,i))) ) ;
      }
  

      /////////////////////
      // step 2: Solve the robust x with the minimum observation
      /////////////////////


      // required number + 1
      int numRequiredObs = (numUnknowns - numApriori) + 1;

      Vector<int> goodVec(pRow, 0);

      numValid = 1;
      for(int i=0; i<pRow; i++)
      {
          int indexMin = getMinIndex(stdResidual, goodVec);

          // we need at least 5 satellite to compute the difference of x and t
          if( numValid <= numRequiredObs )
          {
              goodVec(indexMin) = 1;
              numValid++;
          }
      }

      tempWeight = weightMatrix;
      for(int i=0; i<pRow; i++)
      {
          if(goodVec(i) == 0)
          {
              tempWeight(i,i) = 1.0E-8;
          }
      }

      try
      {
         Matrix<double> invTemp;
         invTemp = ( AT * tempWeight* designMatrix + invPMinus );
         covMatrix = inverseChol( invTemp );

      }
      catch(...)
      {
         InvalidSolver e("MeasUpdate(): Unable to compute P matrix.");
         GPSTK_THROW(e);
         return -1;
      }

      try
      {
          solution = covMatrix * ( (AT * tempWeight * prefitResiduals) + (invPMinus * xhatminus) );
      }
      catch(Exception e)
      {
         InvalidSolver eis("MeasUpdate(): Unable to compute xhat.");
         GPSTK_THROW(eis);
         return -1;
      }

         // Compute the postfit residuals Vector
      postfitResiduals = prefitResiduals - (designMatrix * solution);


      VTPV = 0.0;
      for( int i = 0; i < pRow; ++i)
      {
          VTPV += postfitResiduals(i)*postfitResiduals(i)*tempWeight(i,i);
      }

      Vx = solution - xhatminus;
      VTPVX = (0.0);
      for(int i=0; i< numUnknowns; i++ )
      {
          VTPVX += Vx(i)*invPMinus(i,i)*Vx(i);
      }

      // update the weight
      // warning, the numvalid should be valid number
      sigma = std::sqrt( (VTPV + VTPVX)/(numValid + numApriori - numUnknowns) );


      // warning: here the weightMatrix should be given for all observations
      Qvv = inverseChol(weightMatrix) - designMatrix * covMatrix * AT;
  
      for(int i=0; i<pRow; i++)
      {
          stdResidual[i] =  std::abs( postfitResiduals(i) /(sigma*std::sqrt(Qvv(i,i))) ) ;
      }

      // let's compute the population variance
      tempVTPVAcc = vtpvAcc + VTPV;
      tempNumObsAcc = numObsAcc + numValid - 4;
      totalSigma = std::sqrt(tempVTPVAcc/(tempNumObsAcc));

      Qvv = inverseChol(weightMatrix);
      for(int i=0; i<pRow; i++)
      {
          robustResidual[i] =  std::abs( postfitResiduals(i) /(totalSigma*std::sqrt(Qvv(i,i))) ) ;
      }

      //////////////
      // 3rd, detect the robust outlier using robust stdResidual
      // and solve the equation using the clean observations
      //////////////

      tempWeight = weightMatrix;
      numValid = pRow;

      Vector<int> outlierVec(numEqu, 0);
      for(int i=0; i<pRow; i++)
      {
          if( globalTest )
          {
              if( robustResidual(i) > 4.0 && std::abs( postfitResiduals(i) ) > threshold )
              {
                  outlierVec(i) = 1.0;
                  tempWeight(i,i) = 1.0E-8; 
              }
          }
          else
          {
              if( stdResidual(i) > 4.0 && std::abs( postfitResiduals(i) ) > threshold )
              {
                  outlierVec(i) = 1.0;
                  tempWeight(i,i) = 1.0E-8; 
              }
          }
      }

      try
      {
         Matrix<double> invTemp;
         invTemp = ( AT * tempWeight* designMatrix + invPMinus );
            // Compute the a posteriori error covariance matrix
         covMatrix = inverseChol( invTemp );

      }
      catch(...)
      {
         InvalidSolver e("MeasUpdate(): Unable to compute P matrix.");
         GPSTK_THROW(e);
         return -1;
      }

      try
      {
          solution = covMatrix * ( (AT * tempWeight * prefitResiduals) + (invPMinus * xhatminus) );
      }
      catch(Exception e)
      {
         InvalidSolver eis("MeasUpdate(): Unable to compute xhat.");
         GPSTK_THROW(eis);
         return -1;
      }

         // Compute the postfit residuals Vector
      postfitResiduals = prefitResiduals - (designMatrix * solution);

      VTPV = 0.0;
      for( int i = 0; i < pRow; ++i)
      {
          VTPV += postfitResiduals(i)*postfitResiduals(i)*tempWeight(i,i);
      }

      Vx = solution - xhatminus;
      VTPVX = (0.0);
      for(int i=0; i< numUnknowns; i++ )
      {
          VTPVX += Vx(i)*invPMinus(i,i)*Vx(i);
      }

      // update the weight
      // warning, the numvalid should be valid number
      sigma = std::sqrt( (VTPV + VTPVX)/(numValid + numApriori - numUnknowns) );

      // warning: here the weightMatrix should be given for all observations
      Qvv = inverseChol(weightMatrix) - designMatrix * covMatrix * AT;

      for(int i=0; i<pRow; i++)
      {
          stdResidual[i] =  std::abs( postfitResiduals(i) /(sigma*std::sqrt(Qvv(i,i))) ) ;
      }

      int i = 0;
      for(auto v: currentUnkSet)
      {
          if(v.getType()==TypeID::dStaX)
          {
              deltax.dx = solution(i);
          }
          else if(v.getType()==TypeID::dStaY)
          {
              deltax.dy = solution(i);
          }
          else if(v.getType()==TypeID::dStaZ)
          {
              deltax.dz = solution(i);
          }
          else if(v.getType()==TypeID::dcdtSta)
          {
              deltax.dt = solution(i);
          }

          i++;
      }

      // now, conver solution to delta

      // return the outlier flags
      outlierVector = outlierVec;
      sig = sigma;

      standResidual = stdResidual;

      // store the total number of the freedom
      vtpvAcc += VTPV;
      numObsAcc += numValid - 4;

      return 0;

   }  // End of method 'CSDetectorLC::MeasUpdate()'


      // Correct the state vector and covariance matrix
      //
      // @param gData    Data object holding the data.
      //
   int CSDetectorLC::Robust( Vector<double>& prefitResiduals,
                             Matrix<double>& designMatrix,
                             Matrix<double>& weightMatrix,
                             Vector<double>& postfitResiduals,
                             Vector<double>& standResidual,
                             double& sig,
                             Vector<int>& outlierVector)
       noexcept(false)
   {

         // By default, results are invalid
      if (!(weightMatrix.isSquare()))
      {
         InvalidSolver e("Compute(): Weight matrix is not square");
         cerr << e << endl;
         exit(-1);
      }

      int wRow = static_cast<int>(weightMatrix.rows());
      int pRow = static_cast<int>(prefitResiduals.size());
      if (!(wRow==pRow))
      {
         InvalidSolver e("Compute(): size does not match!");
         cerr << e << endl;
         exit(-1);
      }


      int gRow = static_cast<int>(designMatrix.rows());
      if (!(gRow==pRow))
      {
         InvalidSolver e("Compute(): size does not match !");
         cerr << e << endl;
         exit(-1);
      }

         // After checking sizes, let's do the real correction work
      Matrix<double> AT( transpose(designMatrix) );

      int numEqu( prefitResiduals.size() );
      int numUnknowns( designMatrix.cols() );

      Matrix<double> tempWeight = weightMatrix;

      Vector<double> stdResidual(pRow, 0.0);
      Vector<double> robustResidual(pRow, 0.0);

      double VTPV;
      int numValid = numEqu;
      double sigma;

      try
      {
         Matrix<double> invTemp;
         invTemp = ( AT * tempWeight* designMatrix );
         covMatrix = inverseChol( invTemp );

      }
      catch(...)
      {
         InvalidSolver e("MeasUpdate(): Unable to compute P matrix.");
         GPSTK_THROW(e);
         return -1;
      }

      try
      {
         solution = covMatrix * ( (AT * tempWeight * prefitResiduals) );
      }
      catch(Exception e)
      {
         InvalidSolver eis("MeasUpdate(): Unable to compute xhat.");
         GPSTK_THROW(eis);
         return -1;
      }

         // Compute the postfit residuals Vector
      postfitResiduals = prefitResiduals - (designMatrix * solution);

      VTPV = 0.0;
      for( int i = 0; i < pRow; ++i)
      {
          VTPV += postfitResiduals(i)*postfitResiduals(i)*tempWeight(i,i);
      }

      // update the weight
      // warning, the numvalid should be valid number
      int numRequiredObs;
      numRequiredObs = numUnknowns;
      sigma = std::sqrt( VTPV /(numValid - numUnknowns) );

      // Covariance for observation;
      Matrix<double> Qvv(pRow, pRow, 0.0);

      // warning: here the weightMatrix should be given for all observations
      Qvv = inverseChol(weightMatrix) - designMatrix * covMatrix * AT;

      for(int i=0; i<pRow; i++)
      {
          stdResidual[i] =  std::abs( postfitResiduals(i) /(sigma*std::sqrt(Qvv(i,i))) ) ;
      }
  
      // let's compute the population variance
      double tempVTPVAcc;
      double tempNumObsAcc;

      tempVTPVAcc = vtpvAcc + VTPV;
      tempNumObsAcc = numObsAcc + numValid - 4;

      double totalSigma = std::sqrt(tempVTPVAcc/(tempNumObsAcc));

      //////////////////////
      //
      // the value 2.0 is robust  
      //
      //////////////////////

/*
      Vector<int> badVec(pRow, 0);
      numValid = pRow;
      for(int i=0; i<pRow; i++)
      {
          int indexMax = getMaxIndex(stdResidual, badVec);
          // we need at least 4 satellite to compute the difference of x and t
          if( stdResidual(indexMax) > 2.0 && numValid > 4)
          {
              tempWeight(indexMax, indexMax) = 1.0E-8;
              badVec(indexMax) = 1;
              numValid--;
          }
      }
*/

      Vector<int> goodVec(pRow, 0);
      numValid = 0;
      for(int i=0; i<pRow; i++)
      {
          int indexMin = getMinIndex(stdResidual, goodVec);

          // we need at least 5 satellite to compute the difference of x and t
          if( numValid <= numRequiredObs )
          {
              goodVec(indexMin) = 1;
              numValid++;
          }
      }

      tempWeight = weightMatrix;
      for(int i=0; i<pRow; i++)
      {
          if(goodVec(i) == 0)
          {
              tempWeight(i,i) = 1.0E-8;
          }
      }

      // robust estimation

      /////////////////////
      // Solve the robust x with robust observation
      /////////////////////

      try
      {
         Matrix<double> invTemp;
         invTemp = ( AT * tempWeight* designMatrix );
         covMatrix = inverseChol( invTemp );

      }
      catch(...)
      {
         InvalidSolver e("MeasUpdate(): Unable to compute P matrix.");
         GPSTK_THROW(e);
         return -1;
      }

      try
      {
            // Compute the a posteriori state estimation
          solution = covMatrix * ( (AT * tempWeight * prefitResiduals) );
      }
      catch(Exception e)
      {
         InvalidSolver eis("MeasUpdate(): Unable to compute xhat.");
         GPSTK_THROW(eis);
         return -1;
      }

         // Compute the postfit residuals Vector
      postfitResiduals = prefitResiduals - (designMatrix * solution);

      VTPV = 0.0;
      for( int i = 0; i < pRow; ++i)
      {
          VTPV += postfitResiduals(i)*postfitResiduals(i)*tempWeight(i,i);
      }

      sigma = std::sqrt( VTPV /(numValid - numUnknowns) );

      // warning: here the weightMatrix should be given for all observations
  
      Qvv = inverseChol(weightMatrix) - designMatrix * covMatrix * AT;
  
      Qvv = inverseChol(weightMatrix);

      for(int i=0; i<pRow; i++)
      {
          stdResidual[i] =  std::abs( postfitResiduals(i) /(sigma*std::sqrt(Qvv(i,i))) ) ;
      }

      //////////////
      // at last, decide the robust outlier using robust stdResidual
      // and solve the equation using the clean observations
      //////////////

      tempWeight = weightMatrix;
      numValid = pRow;

      Vector<int> outlierVec(numEqu, 0);
      for(int i=0; i<pRow; i++)
      {
          if( stdResidual(i) > 3.5 && std::abs( postfitResiduals(i) ) > threshold )
          {
              outlierVec(i) = 1.0;
              tempWeight(i,i) = 1.0E-8; 
          }
      }

      try
      {
         Matrix<double> invTemp;
         invTemp = ( AT * tempWeight* designMatrix );
         covMatrix = inverseChol( invTemp );

      }
      catch(...)
      {
         InvalidSolver e("MeasUpdate(): Unable to compute P matrix.");
         GPSTK_THROW(e);
         return -1;
      }

      try
      {
            // Compute the a posteriori state estimation
          solution = covMatrix * ( (AT * tempWeight * prefitResiduals) );
      }
      catch(Exception e)
      {
         InvalidSolver eis("MeasUpdate(): Unable to compute xhat.");
         GPSTK_THROW(eis);
         return -1;
      }

         // Compute the postfit residuals Vector
      postfitResiduals = prefitResiduals - (designMatrix * solution);


      VTPV = 0.0;
      for( int i = 0; i < pRow; ++i)
      {
          VTPV += postfitResiduals(i)*postfitResiduals(i)*tempWeight(i,i);
      }

      // update the weight
      // warning, the numvalid should be valid number
      sigma = std::sqrt( VTPV /(numValid - numUnknowns) );

      // warning: here the weightMatrix should be given for all observations
      Qvv = inverseChol(weightMatrix) - designMatrix * covMatrix * AT;

      for(int i=0; i<pRow; i++)
      {
          stdResidual[i] =  std::abs( postfitResiduals(i) /(sigma*std::sqrt(Qvv(i,i))) ) ;
      }

      if(debug)
      {
          cout << "stdResidual" << endl;
          cout << stdResidual << endl;
      }

      int i = 0;
      for(auto v: currentUnkSet)
      {
          if(v.getType()==TypeID::dStaX)
          {
              deltax.dx = solution(i);
          }
          else if(v.getType()==TypeID::dStaY)
          {
              deltax.dy = solution(i);
          }
          else if(v.getType()==TypeID::dStaZ)
          {
              deltax.dz = solution(i);
          }
          else if(v.getType()==TypeID::dcdtSta)
          {
              deltax.dt = solution(i);
          }

          i++;
      }

      // now, conver solution to delta

      // return the outlier flags
      outlierVector = outlierVec;
      sig = sigma;

      standResidual = stdResidual;

      // store the total number of the freedom
      vtpvAcc += VTPV;
      numObsAcc += numValid - 4;

      return 0;

   }  // End of method 'CSDetectorLC::MeasUpdate()'


   int CSDetectorLC::getMaxIndex(Vector<double>& residual, 
                                 Vector<int>& flagVec)
   {
       double max=0.0;
       int indexMax = 0;
       for(int i=0; i<residual.size(); i++)
       {
           if(std::abs(residual(i))>max && flagVec(i) !=1 )
           {
               max = std::abs(residual(i));
               indexMax = i;
           }
       }
       return indexMax;
   };


   int CSDetectorLC::getMinIndex(Vector<double>& residual, 
                                 Vector<int>& flagVec)
   {
       double min= 1.0E+8;
       int minIndex = 0;
       for(int i=0; i<residual.size(); i++)
       {
           if(std::abs(residual(i)) < min && flagVec(i) !=1 )
           {
               min = std::abs(residual(i));
               minIndex = i;
           }
       }
       return minIndex;
   };



   void CSDetectorLC::getEpochDiffData(gnssRinex& gRin)
       noexcept(false)
   {
       try
       {
          // loop the satellite
          for(auto& stv: gRin.body)
          {
              SatID sat = stv.first;

              if(stvDataPrev.find(sat) != stvDataPrev.end())
              {
                  typeValueMap tvDataPrev = stvDataPrev[sat];
                  
                  if(sat.system == SatelliteSystem::GPS )
                  {
                      for(auto type: gpsCombTypeSet)
                      {
                          if(type==TypeID::LC12)
                          {
                              bool hasCurrPrefit(false);
                              if( stv.second.find(TypeID::prefitL12) != stv.second.end() )
                              {
                                  hasCurrPrefit = true;;
                              }

                              bool hasPrevPrefit(false);
                              if( tvDataPrev.find(TypeID::prefitL12) != tvDataPrev.end() )
                              {
                                  hasPrevPrefit = true;;
                              }

                              double currPrefit, prevPrefit;
                              if(hasCurrPrefit && hasPrevPrefit)
                              {
                                  currPrefit = stv.second(TypeID::prefitL12);
                                  prevPrefit = tvDataPrev(TypeID::prefitL12); 
                                  stv.second[TypeID::diffPrefitL12] = currPrefit - prevPrefit;
                              }

                              if( hasCurrPrefit && !hasPrevPrefit )
                              {
                                  // data break happened
                                  stv.second[TypeID::CSLC] = 9.0;
                              }

                          }
                          else if( type == TypeID::LC15)
                          {
                              bool hasCurrPrefit(false);
                              if( stv.second.find(TypeID::prefitL15) != stv.second.end() )
                              {
                                  hasCurrPrefit = true;;
                              }

                              bool hasPrevPrefit(false);
                              if( tvDataPrev.find(TypeID::prefitL15) != tvDataPrev.end() )
                              {
                                  hasPrevPrefit = true;;
                              }

                              double currPrefit, prevPrefit;
                              if(hasCurrPrefit && hasPrevPrefit)
                              {
                                  currPrefit = stv.second(TypeID::prefitL15);
                                  prevPrefit = tvDataPrev(TypeID::prefitL15); 
                                  stv.second[TypeID::diffPrefitL15] = currPrefit - prevPrefit;
                              }

                              if( hasCurrPrefit && !hasPrevPrefit )
                              {
                                  // data break happened
                                  stv.second[TypeID::CSLC] = 9.0;
                              }
                          }

                      }
                  }
                  else if(sat.system == SatelliteSystem::Galileo)
                  {
                      for(auto type: galCombTypeSet)
                      {
                          if( type == TypeID::LC15)
                          {
                              bool hasCurrPrefit(false);
                              if( stv.second.find(TypeID::prefitL15) != stv.second.end() )
                              {
                                  hasCurrPrefit = true;;
                              }

                              bool hasPrevPrefit(false);
                              if( tvDataPrev.find(TypeID::prefitL15) != tvDataPrev.end() )
                              {
                                  hasPrevPrefit = true;;
                              }

                              double currPrefit, prevPrefit;
                              if(hasCurrPrefit && hasPrevPrefit)
                              {
                                  currPrefit = stv.second(TypeID::prefitL15);
                                  prevPrefit = tvDataPrev(TypeID::prefitL15); 
                                  stv.second[TypeID::diffPrefitL15] = currPrefit - prevPrefit;
                              }

                              if( hasCurrPrefit && !hasPrevPrefit )
                              {
                                  // data break happened
                                  stv.second[TypeID::CSLC] = 9.0;
                              }
                          }
                          else if( type == TypeID::LC17)
                          {
                              bool hasCurrPrefit(false);
                              if( stv.second.find(TypeID::prefitL17) != stv.second.end() )
                              {
                                  hasCurrPrefit = true;;
                              }

                              bool hasPrevPrefit(false);
                              if( tvDataPrev.find(TypeID::prefitL17) != tvDataPrev.end() )
                              {
                                  hasPrevPrefit = true;;
                              }

                              double currPrefit, prevPrefit;
                              if(hasCurrPrefit && hasPrevPrefit)
                              {
                                  currPrefit = stv.second(TypeID::prefitL17);
                                  prevPrefit = tvDataPrev(TypeID::prefitL17); 
                                  stv.second[TypeID::diffPrefitL17] = currPrefit - prevPrefit;
                              }

                              if( hasCurrPrefit && !hasPrevPrefit )
                              {
                                  // data break happened
                                  stv.second[TypeID::CSLC] = 9.0;
                              }
                          }
                          else if( type == TypeID::LC18)
                          {
                              bool hasCurrPrefit(false);
                              if( stv.second.find(TypeID::prefitL18) != stv.second.end() )
                              {
                                  hasCurrPrefit = true;;
                              }

                              bool hasPrevPrefit(false);
                              if( tvDataPrev.find(TypeID::prefitL18) != tvDataPrev.end() )
                              {
                                  hasPrevPrefit = true;;
                              }

                              double currPrefit, prevPrefit;
                              if(hasCurrPrefit && hasPrevPrefit)
                              {
                                  currPrefit = stv.second(TypeID::prefitL18);
                                  prevPrefit = tvDataPrev(TypeID::prefitL18); 
                                  stv.second[TypeID::diffPrefitL18] = currPrefit - prevPrefit;
                              }

                              if( hasCurrPrefit && !hasPrevPrefit )
                              {
                                  // data break happened
                                  stv.second[TypeID::CSLC] = 9.0;
                              }
                          }
                          else if( type == TypeID::LC16)
                          {
                              bool hasCurrPrefit(false);
                              if( stv.second.find(TypeID::prefitL16) != stv.second.end() )
                              {
                                  hasCurrPrefit = true;;
                              }

                              bool hasPrevPrefit(false);
                              if( tvDataPrev.find(TypeID::prefitL16) != tvDataPrev.end() )
                              {
                                  hasPrevPrefit = true;;
                              }

                              double currPrefit, prevPrefit;
                              if(hasCurrPrefit && hasPrevPrefit)
                              {
                                  currPrefit = stv.second(TypeID::prefitL16);
                                  prevPrefit = tvDataPrev(TypeID::prefitL16); 
                                  stv.second[TypeID::diffPrefitL16] = currPrefit - prevPrefit;
                              }

                              if( hasCurrPrefit && !hasPrevPrefit )
                              {
                                  // data break happened
                                  stv.second[TypeID::CSLC] = 9.0;
                              }
                          }
                      }
                  }
                  else if(sat.system == SatelliteSystem::BDS)
                  {
                      for(auto type: bdsCombTypeSet)
                      {
                          if( type == TypeID::LC26)
                          {
                              bool hasCurrPrefit(false);
                              if( stv.second.find(TypeID::prefitL26) != stv.second.end() )
                              {
                                  hasCurrPrefit = true;;
                              }

                              bool hasPrevPrefit(false);
                              if( tvDataPrev.find(TypeID::prefitL26) != tvDataPrev.end() )
                              {
                                  hasPrevPrefit = true;;
                              }

                              double currPrefit, prevPrefit;
                              if(hasCurrPrefit && hasPrevPrefit)
                              {
                                  currPrefit = stv.second(TypeID::prefitL26);
                                  prevPrefit = tvDataPrev(TypeID::prefitL26); 
                                  stv.second[TypeID::diffPrefitL26] = currPrefit - prevPrefit;
                              }

                              if( hasCurrPrefit && !hasPrevPrefit )
                              {
                                  // data break happened
                                  stv.second[TypeID::CSLC] = 9.0;
                              }
                          }
                          else if( type == TypeID::LC27)
                          {
                              bool hasCurrPrefit(false);
                              if( stv.second.find(TypeID::prefitL27) != stv.second.end() )
                              {
                                  hasCurrPrefit = true;;
                              }

                              bool hasPrevPrefit(false);
                              if( tvDataPrev.find(TypeID::prefitL27) != tvDataPrev.end() )
                              {
                                  hasPrevPrefit = true;;
                              }

                              double currPrefit, prevPrefit;
                              if(hasCurrPrefit && hasPrevPrefit)
                              {
                                  currPrefit = stv.second(TypeID::prefitL27);
                                  prevPrefit = tvDataPrev(TypeID::prefitL27); 
                                  stv.second[TypeID::diffPrefitL27] = currPrefit - prevPrefit;
                              }

                              if( hasCurrPrefit && !hasPrevPrefit )
                              {
                                  // data break happened
                                  stv.second[TypeID::CSLC] = 9.0;
                              }
                          }
                      }
                  }
                  else if(sat.system == SatelliteSystem::GLONASS)
                  {
                      // to be done
                  }

              }
              else
              {
                  if(sat.system==SatelliteSystem::GPS)
                  {
                      stv.second[TypeID::CSLC] = 9.0;
                  }
                  else if(sat.system==SatelliteSystem::Galileo)
                  {
                      stv.second[TypeID::CSLC] = 9.0;
                  }
                  else if(sat.system==SatelliteSystem::BDS)
                  {
                      stv.second[TypeID::CSLC] = 9.0;
                  }
                  else if(sat.system==SatelliteSystem::GLONASS)
                  {
                      // to be done
                  }
              }
          }
       }
       catch(Exception& u)
       {
             // Throw an exception if something unexpected happens
          ProcessingException e( getClassName() + ":"
                                 + StringUtils::asString( getIndex() ) + ":"
                                 + u.what() );

          GPSTK_THROW(e);

       }

       return;
   }

   Variable CSDetectorLC::getVariable( const TypeID& type,
                                       const VariableSet& varSet) const
      noexcept(false)
   {

         // Declare an iterator for 'stateMap' and go to the first element
      VariableSet::const_iterator it = varSet.begin();

         // Look for a variable with the same type
      while( (*it).getType() != type &&
             it != varSet.end() )
      {
         ++it;

         // If the same type is not found, throw an exception
         if( it == varSet.end() )
         {
             InvalidRequest e("Type not found in varSet.");
             GPSTK_THROW(e);
         }
      }

         // Else, return the corresponding value
      return (*it);

   }  // End of method 'SolverGeneral::getSolution()'


}  // End of namespace gpstk
